using BepuPhysics.Collidables;
using BepuUtilities;
using System;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.CollisionDetection.CollisionTasks
{
    using DepthRefiner = DepthRefiner<ConvexHull, ConvexHullWide, ConvexHullSupportFinder, Triangle, TriangleWide, PretransformedTriangleSupportFinder>;
    public struct TriangleConvexHullTester : IPairTester<TriangleWide, ConvexHullWide, Convex4ContactManifoldWide>
    {
        public int BatchSize => 16;

        public unsafe void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold)
        {
            manifold = default;
            Matrix3x3Wide.CreateFromQuaternion(orientationA, out var triangleOrientation);
            Matrix3x3Wide.CreateFromQuaternion(orientationB, out var hullOrientation);
            Matrix3x3Wide.MultiplyByTransposeWithoutOverlap(triangleOrientation, hullOrientation, out var hullLocalTriangleOrientation);

            Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, hullOrientation, out var localOffsetB);
            Vector3Wide.Negate(localOffsetB, out var localOffsetA);

            TriangleWide triangle;
            Matrix3x3Wide.TransformWithoutOverlap(a.A, hullLocalTriangleOrientation, out triangle.A);
            Matrix3x3Wide.TransformWithoutOverlap(a.B, hullLocalTriangleOrientation, out triangle.B);
            Matrix3x3Wide.TransformWithoutOverlap(a.C, hullLocalTriangleOrientation, out triangle.C);
            Vector3Wide.Add(triangle.A, triangle.B, out var centroid);
            Vector3Wide.Add(triangle.C, centroid, out centroid);
            Vector3Wide.Scale(centroid, new Vector<float>(1f / 3f), out centroid);
            Vector3Wide.Subtract(triangle.A, centroid, out triangle.A);
            Vector3Wide.Subtract(triangle.B, centroid, out triangle.B);
            Vector3Wide.Subtract(triangle.C, centroid, out triangle.C);
            Vector3Wide.Subtract(centroid, localOffsetB, out var localTriangleCenter);
            Vector3Wide.Subtract(triangle.B, triangle.A, out var triangleAB);
            Vector3Wide.Subtract(triangle.C, triangle.B, out var triangleBC);
            Vector3Wide.Subtract(triangle.A, triangle.C, out var triangleCA);
            // 我们将大量使用B局部三角形顶点,因此请缓存它们。
            Vector3Wide.Add(triangle.A, localTriangleCenter, out var triangleA);
            Vector3Wide.Add(triangle.B, localTriangleCenter, out var triangleB);
            Vector3Wide.Add(triangle.C, localTriangleCenter, out var triangleC);
            Vector3Wide.CrossWithoutOverlap(triangleAB, triangleCA, out var triangleNormal);
            Vector3Wide.Length(triangleNormal, out var triangleNormalLength);
            Vector3Wide.Scale(triangleNormal, Vector<float>.One / triangleNormalLength, out triangleNormal);

            // 检查船体的位置是否在三角形内且低于三角形平面。如果是这样的话,我们可以忽略它。
            Vector3Wide.Dot(triangleNormal, localTriangleCenter, out var hullToTriangleCenterDot);
            var hullBelowPlane = Vector.GreaterThanOrEqual(hullToTriangleCenterDot, Vector<float>.Zero);
            Vector3Wide.CrossWithoutOverlap(triangleAB, triangleNormal, out var edgePlaneAB);
            Vector3Wide.CrossWithoutOverlap(triangleBC, triangleNormal, out var edgePlaneBC);
            Vector3Wide.CrossWithoutOverlap(triangleCA, triangleNormal, out var edgePlaneCA);
            Vector3Wide.Dot(edgePlaneAB, triangleA, out var abPlaneTest);
            Vector3Wide.Dot(edgePlaneBC, triangleB, out var bcPlaneTest);
            Vector3Wide.Dot(edgePlaneCA, triangleC, out var caPlaneTest);
            var hullInsideTriangleEdgePlanes = Vector.BitwiseAnd(Vector.LessThanOrEqual(abPlaneTest, Vector<float>.Zero),
                    Vector.BitwiseAnd(Vector.LessThanOrEqual(bcPlaneTest, Vector<float>.Zero), Vector.LessThanOrEqual(caPlaneTest, Vector<float>.Zero)));
            var hullInsideAndBelowTriangle = Vector.BitwiseAnd(hullBelowPlane, hullInsideTriangleEdgePlanes);


            ManifoldCandidateHelper.CreateInactiveMask(pairCount, out var inactiveLanes);
            a.EstimateEpsilonScale(out var triangleEpsilonScale);
            b.EstimateEpsilonScale(inactiveLanes, out var hullEpsilonScale);
            var epsilonScale = Vector.Min(triangleEpsilonScale, hullEpsilonScale);
            inactiveLanes = Vector.BitwiseOr(inactiveLanes, Vector.LessThan(triangleNormalLength, epsilonScale * 1e-6f));
            inactiveLanes = Vector.BitwiseOr(inactiveLanes, hullInsideAndBelowTriangle);
            // 并不是每条车道都会产生触点。不必要求每条车道都仔细清除所有的退出联系人状态,只需在前面清除它们即可。
            manifold.Contact0Exists = default;
            manifold.Contact1Exists = default;
            manifold.Contact2Exists = default;
            manifold.Contact3Exists = default;
            if (Vector.LessThanAll(inactiveLanes, Vector<int>.Zero))
            {
                // 未生成联系人。
                return;
            }

            // 请注意,使用三角形中心作为初始法线,而不是使用localOffsetA。
            // 不保证三角形以其质心为中心,并且DepthRefiner
            // 将收敛到不与迄今最好的法线相反的深度-在早期迭代中,
            // 可能是最初的正常情况。
            Vector3Wide.Length(localTriangleCenter, out var centerDistance);
            Vector3Wide.Scale(localTriangleCenter, Vector<float>.One / centerDistance, out var initialNormal);
            var useInitialFallback = Vector.LessThan(centerDistance, new Vector<float>(1e-10f));
            initialNormal.X = Vector.ConditionalSelect(useInitialFallback, Vector<float>.Zero, initialNormal.X);
            initialNormal.Y = Vector.ConditionalSelect(useInitialFallback, Vector<float>.One, initialNormal.Y);
            initialNormal.Z = Vector.ConditionalSelect(useInitialFallback, Vector<float>.Zero, initialNormal.Z);

            // 检查外壳沿其面法线朝向三角形的端点是否位于三角形内部。
            // 如果是这样的话,就不需要深度细化了。
            var hullSupportFinder = default(ConvexHullSupportFinder);
            var triangleSupportFinder = default(PretransformedTriangleSupportFinder);
            // 沿着三角形面法向采样船体的极值点-如果它包含在三角形边缘平面内,我们可以避免进行更昂贵的细化。
            Vector3Wide.Negate(triangleNormal, out var negatedTriangleNormal);
            hullSupportFinder.ComputeLocalSupport(b, negatedTriangleNormal, inactiveLanes, out var hullSupportAlongNegatedTriangleNormal);
            Vector3Wide.Subtract(hullSupportAlongNegatedTriangleNormal, localTriangleCenter, out var supportAlongNegatedTriangleNormal);
            Vector3Wide.Dot(supportAlongNegatedTriangleNormal, negatedTriangleNormal, out var triangleFaceDepth);
            Vector3Wide.Subtract(triangleA, hullSupportAlongNegatedTriangleNormal, out var closestToA);
            Vector3Wide.Subtract(triangleB, hullSupportAlongNegatedTriangleNormal, out var closestToB);
            Vector3Wide.Subtract(triangleC, hullSupportAlongNegatedTriangleNormal, out var closestToC);
            Vector3Wide.Dot(edgePlaneAB, closestToA, out var extremeABPlaneTest);
            Vector3Wide.Dot(edgePlaneBC, closestToB, out var extremeBCPlaneTest);
            Vector3Wide.Dot(edgePlaneCA, closestToC, out var extremeCAPlaneTest);
            // 请注意,只有当外壳的中心位于三角形曲面之上*且*包含在边缘法线内时,三角形面极值点才是可信的。
            // 仅仅在表面之上是不够的-想象一个船体位于三角形的一侧,楔入它的下面。
            var triangleNormalIsMinimal = Vector.BitwiseAnd(
                Vector.BitwiseAnd(
                    Vector.AndNot(hullInsideTriangleEdgePlanes, hullBelowPlane),
                    Vector.LessThanOrEqual(extremeABPlaneTest, Vector<float>.Zero)),
                Vector.BitwiseAnd(
                    Vector.LessThanOrEqual(extremeBCPlaneTest, Vector<float>.Zero),
                    Vector.LessThanOrEqual(extremeCAPlaneTest, Vector<float>.Zero)));

            var depthThreshold = -speculativeMargin;
            var skipDepthRefine = Vector.BitwiseOr(triangleNormalIsMinimal, inactiveLanes);
            Vector3Wide localNormal, closestOnHull;
            Vector<float> depth;
            if (Vector.EqualsAny(skipDepthRefine, Vector<int>.Zero))
            {
                DepthRefiner.FindMinimumDepth(
                    b, triangle, localTriangleCenter, hullLocalTriangleOrientation, ref hullSupportFinder, ref triangleSupportFinder, initialNormal, skipDepthRefine, 1e-4f * epsilonScale, depthThreshold,
                    out var refinedDepth, out var refinedNormal, out var refinedClosestOnHull);
                Vector3Wide.ConditionalSelect(skipDepthRefine, hullSupportAlongNegatedTriangleNormal, refinedClosestOnHull, out closestOnHull);
                Vector3Wide.ConditionalSelect(skipDepthRefine, negatedTriangleNormal, refinedNormal, out localNormal);
                depth = Vector.ConditionalSelect(skipDepthRefine, triangleFaceDepth, refinedDepth);

            }
            else
            {
                // 没有深度精炼运行;极值点PrePass做了我们需要的一切。只需使用初始法线即可。
                localNormal = negatedTriangleNormal;
                closestOnHull = hullSupportAlongNegatedTriangleNormal;
                depth = triangleFaceDepth;
            }


            Vector3Wide.Dot(triangleNormal, localNormal, out var triangleNormalDotLocalNormal);
            inactiveLanes = Vector.BitwiseOr(inactiveLanes, Vector.BitwiseOr(Vector.GreaterThanOrEqual(triangleNormalDotLocalNormal, Vector<float>.Zero), Vector.LessThan(depth, depthThreshold)));
            if (Vector.LessThanAll(inactiveLanes, Vector<int>.Zero))
            {
                // 未生成联系人。
                return;
            }

            // 为了找到接触流形,我们将像往常一样在船体面上修剪三角形边,但我们要处理的可能是
            // 截然不同的凸面船体。我们不是在不同的船体上矢量化,而是在每个船体内矢量化。
            Helpers.FillVectorWithLaneIndices(out var slotOffsetIndices);
            var boundingPlaneEpsilon = 1e-3f * epsilonScale;
            // 接触不能超过6个(假设没有数字错误);每条三角形边2个。
            var candidates = stackalloc ManifoldCandidateScalar[6];
            for (int slotIndex = 0; slotIndex < pairCount; ++slotIndex)
            {
                if (inactiveLanes[slotIndex] < 0)
                    continue;
                ref var hull = ref b.Hulls[slotIndex];
                ConvexHullTestHelper.PickRepresentativeFace(ref hull, slotIndex, ref localNormal, closestOnHull, slotOffsetIndices, ref boundingPlaneEpsilon, out var slotFaceNormal, out var slotLocalNormal, out var bestFaceIndex);

                // 对照船体表面测试每条三角形边。
                // 请注意,我们不使用faceNormal x edgeOffset边平面,而是使用edgeOffset x localNormal。
                // 这些面在右手坐标中逆时针缠绕。
                // 请注意,三角形边被打包到向量4中。从历史上看,Vector3有一些较小的编解码器问题。
                // 可能已经无关紧要了,但是使用死槽是免费的。
                ref var aSlot = ref GatherScatter.GetOffsetInstance(ref triangleA, slotIndex);
                ref var bSlot = ref GatherScatter.GetOffsetInstance(ref triangleB, slotIndex);
                ref var cSlot = ref GatherScatter.GetOffsetInstance(ref triangleC, slotIndex);
                ref var abSlot = ref GatherScatter.GetOffsetInstance(ref triangleAB, slotIndex);
                ref var bcSlot = ref GatherScatter.GetOffsetInstance(ref triangleBC, slotIndex);
                ref var caSlot = ref GatherScatter.GetOffsetInstance(ref triangleCA, slotIndex);
                var triangleEdgeStartX = new Vector4(aSlot.X[0], bSlot.X[0], cSlot.X[0], 0);
                var triangleEdgeStartY = new Vector4(aSlot.Y[0], bSlot.Y[0], cSlot.Y[0], 0);
                var triangleEdgeStartZ = new Vector4(aSlot.Z[0], bSlot.Z[0], cSlot.Z[0], 0);
                var edgeDirectionX = new Vector4(abSlot.X[0], bcSlot.X[0], caSlot.X[0], 0);
                var edgeDirectionY = new Vector4(abSlot.Y[0], bcSlot.Y[0], caSlot.Y[0], 0);
                var edgeDirectionZ = new Vector4(abSlot.Z[0], bcSlot.Z[0], caSlot.Z[0], 0);

                var slotLocalNormalX = new Vector4(slotLocalNormal.X);
                var slotLocalNormalY = new Vector4(slotLocalNormal.Y);
                var slotLocalNormalZ = new Vector4(slotLocalNormal.Z);

                // 边缘平面法线=边缘方向x局部法线
                var triangleEdgePlaneNormalX = edgeDirectionY * slotLocalNormalZ - edgeDirectionZ * slotLocalNormalY;
                var triangleEdgePlaneNormalY = edgeDirectionZ * slotLocalNormalX - edgeDirectionX * slotLocalNormalZ;
                var triangleEdgePlaneNormalZ = edgeDirectionX * slotLocalNormalY - edgeDirectionY * slotLocalNormalX;

                hull.GetVertexIndicesForFace(bestFaceIndex, out var faceVertexIndices);
                var previousIndex = faceVertexIndices[faceVertexIndices.Length - 1];
                Vector3Wide.ReadSlot(ref hull.Points[previousIndex.BundleIndex], previousIndex.InnerIndex, out var hullFaceOrigin);
                var previousVertex = hullFaceOrigin;
                var candidateCount = 0;
                Helpers.BuildOrthnormalBasis(slotFaceNormal, out var hullFaceX, out var hullFaceY);
                Vector4 maximumVertexContainmentDots = Vector4.Zero;
                for (int i = 0; i < faceVertexIndices.Length; ++i)
                {
                    var index = faceVertexIndices[i];
                    Vector3Wide.ReadSlot(ref hull.Points[index.BundleIndex], index.InnerIndex, out var vertex);

                    var hullEdgeOffset = vertex - previousVertex;

                    var hullEdgeStartX = new Vector4(previousVertex.X);
                    var hullEdgeStartY = new Vector4(previousVertex.Y);
                    var hullEdgeStartZ = new Vector4(previousVertex.Z);
                    var hullEdgeOffsetX = new Vector4(hullEdgeOffset.X);
                    var hullEdgeOffsetY = new Vector4(hullEdgeOffset.Y);
                    var hullEdgeOffsetZ = new Vector4(hullEdgeOffset.Z);
                    // 通过对照船体的边平面法线检查三角形顶点的符号来测试三角形顶点的容纳性。
                    // 船体边在右手坐标中逆时针缠绕;边平面法线指向外。
                    // vertexOutside EdgePlane=dot(hullEdgeOffset x slotLocalNormal,triangleVertex-hullEdgeStart)>0
                    var hullEdgePlaneNormal = Vector3.Cross(hullEdgeOffset, slotLocalNormal);
                    var hullEdgePlaneNormalX = new Vector4(hullEdgePlaneNormal.X);
                    var hullEdgePlaneNormalY = new Vector4(hullEdgePlaneNormal.Y);
                    var hullEdgePlaneNormalZ = new Vector4(hullEdgePlaneNormal.Z);
                    var hullEdgeStartToTriangleEdgeX = triangleEdgeStartX - hullEdgeStartX;
                    var hullEdgeStartToTriangleEdgeY = triangleEdgeStartY - hullEdgeStartY;
                    var hullEdgeStartToTriangleEdgeZ = triangleEdgeStartZ - hullEdgeStartZ;
                    var triangleVertexContainmentDots = hullEdgePlaneNormalX * hullEdgeStartToTriangleEdgeX + hullEdgePlaneNormalY * hullEdgeStartToTriangleEdgeY + hullEdgePlaneNormalZ * hullEdgeStartToTriangleEdgeZ;
                    maximumVertexContainmentDots = Vector4.Max(maximumVertexContainmentDots, triangleVertexContainmentDots);
                    // t=点(pointOnTriangleEdge-hullEdgeStart,edgePlane Normal)/dot(edgePlane Normal,hullEdgeOffset)
                    var numerator = hullEdgeStartToTriangleEdgeX * triangleEdgePlaneNormalX + hullEdgeStartToTriangleEdgeY * triangleEdgePlaneNormalY + hullEdgeStartToTriangleEdgeZ * triangleEdgePlaneNormalZ;
                    // 因为我们对分母的符号很敏感,所以三角形边的缠绕很重要。
                    var denominator = triangleEdgePlaneNormalX * hullEdgeOffsetX + triangleEdgePlaneNormalY * hullEdgeOffsetY + triangleEdgePlaneNormalZ * hullEdgeOffsetZ;
                    var edgeIntersections = numerator / denominator;


                    // 如果光线方向与面法线相反,则表示平面被"进入"。
                    // 进入分母始终为负,退出分母始终为正。不用担心比较符号翻转。
                    float latestEntry, earliestExit;
                    if (denominator.X < 0)
                    {
                        latestEntry = edgeIntersections.X;
                        earliestExit = float.MaxValue;
                    }
                    else if (denominator.X > 0)
                    {
                        latestEntry = float.MinValue;
                        earliestExit = edgeIntersections.X;
                    }
                    else
                    {
                        latestEntry = float.MinValue;
                        earliestExit = float.MaxValue;
                    }
                    if (denominator.Y < 0)
                    {
                        if (edgeIntersections.Y > latestEntry)
                            latestEntry = edgeIntersections.Y;
                    }
                    else if (denominator.Y > 0)
                    {
                        if (edgeIntersections.Y < earliestExit)
                            earliestExit = edgeIntersections.Y;
                    }
                    if (denominator.Z < 0)
                    {
                        if (edgeIntersections.Z > latestEntry)
                            latestEntry = edgeIntersections.Z;
                    }
                    else if (denominator.Z > 0)
                    {
                        if (edgeIntersections.Z < earliestExit)
                            earliestExit = edgeIntersections.Z;
                    }

                    // 现在我们有了凸包边间隔。为其添加联系人。
                    latestEntry = latestEntry < 0 ? 0 : latestEntry;
                    earliestExit = earliestExit > 1 ? 1 : earliestExit;
                    // 如果max>=min,则创建最大联系人。
                    // 如果MIN<max且MIN>0,则创建MIN。
                    var startId = (previousIndex.BundleIndex << BundleIndexing.VectorShift) + previousIndex.InnerIndex;
                    var endId = (index.BundleIndex << BundleIndexing.VectorShift) + index.InnerIndex;
                    var baseFeatureId = (startId ^ endId) << 8;
                    if (earliestExit >= latestEntry && candidateCount < 6)
                    {
                        // 创建最大联系人数。
                        var point = hullEdgeOffset * earliestExit + previousVertex - hullFaceOrigin;
                        var newContactIndex = candidateCount++;
                        ref var candidate = ref candidates[newContactIndex];
                        candidate.X = Vector3.Dot(point, hullFaceX);
                        candidate.Y = Vector3.Dot(point, hullFaceY);
                        candidate.FeatureId = baseFeatureId + endId;

                    }
                    if (latestEntry < earliestExit && latestEntry > 0 && candidateCount < 6)
                    {
                        // 创建最小联系人。
                        var point = hullEdgeOffset * latestEntry + previousVertex - hullFaceOrigin;
                        var newContactIndex = candidateCount++;
                        ref var candidate = ref candidates[newContactIndex];
                        candidate.X = Vector3.Dot(point, hullFaceX);
                        candidate.Y = Vector3.Dot(point, hullFaceY);
                        candidate.FeatureId = baseFeatureId + startId;

                    }

                    previousIndex = index;
                    previousVertex = vertex;
                }
                if (candidateCount < 6)
                {
                    // 尝试添加三角形顶点接触。将每个顶点投影到外壳面。
                    // t=点(triangleVertex-hullFaceVertex,hullFacePlane Normal)/点(hullFacePlane Normal,localNormal)
                    var closestOnHullX = new Vector4(hullFaceOrigin.X);
                    var closestOnHullY = new Vector4(hullFaceOrigin.Y);
                    var closestOnHullZ = new Vector4(hullFaceOrigin.Z);
                    var hullFaceNormalX = new Vector4(slotFaceNormal.X);
                    var hullFaceNormalY = new Vector4(slotFaceNormal.Y);
                    var hullFaceNormalZ = new Vector4(slotFaceNormal.Z);
                    var closestOnHullToTriangleEdgeStartX = triangleEdgeStartX - closestOnHullX;
                    var closestOnHullToTriangleEdgeStartY = triangleEdgeStartY - closestOnHullY;
                    var closestOnHullToTriangleEdgeStartZ = triangleEdgeStartZ - closestOnHullZ;
                    var vertexProjectionNumerator = (closestOnHullToTriangleEdgeStartX) * hullFaceNormalX + (closestOnHullToTriangleEdgeStartY) * hullFaceNormalY + (closestOnHullToTriangleEdgeStartZ) * hullFaceNormalZ;
                    var vertexProjectionDenominator = new Vector4(Vector3.Dot(slotFaceNormal, slotLocalNormal));
                    var vertexProjectionT = vertexProjectionNumerator / vertexProjectionDenominator;
                    // 从B到A的法点。
                    var projectedVertexX = closestOnHullToTriangleEdgeStartX - vertexProjectionT * slotLocalNormalX;
                    var projectedVertexY = closestOnHullToTriangleEdgeStartY - vertexProjectionT * slotLocalNormalY;
                    var projectedVertexZ = closestOnHullToTriangleEdgeStartZ - vertexProjectionT * slotLocalNormalZ;
                    var hullFaceXX = new Vector4(hullFaceX.X);
                    var hullFaceXY = new Vector4(hullFaceX.Y);
                    var hullFaceXZ = new Vector4(hullFaceX.Z);
                    var hullFaceYX = new Vector4(hullFaceY.X);
                    var hullFaceYY = new Vector4(hullFaceY.Y);
                    var hullFaceYZ = new Vector4(hullFaceY.Z);
                    var projectedTangentX = projectedVertexX * hullFaceXX + projectedVertexY * hullFaceXY + projectedVertexZ * hullFaceXZ;
                    var projectedTangentY = projectedVertexX * hullFaceYX + projectedVertexY * hullFaceYY + projectedVertexZ * hullFaceYZ;
                    // 我们采用了所有三角杠杆点-外壳平面测试中的最大值;如果顶点在任何边平面之外,则最大点将为正。
                    if (maximumVertexContainmentDots.X <= 0)
                    {
                        ref var candidate = ref candidates[candidateCount++];
                        candidate.X = projectedTangentX.X;
                        candidate.Y = projectedTangentY.X;
                        candidate.FeatureId = 0;
                    }
                    if (candidateCount == 6)
                        goto SkipVertexCandidates;
                    if (maximumVertexContainmentDots.Y <= 0)
                    {
                        ref var candidate = ref candidates[candidateCount++];
                        candidate.X = projectedTangentX.Y;
                        candidate.Y = projectedTangentY.Y;
                        candidate.FeatureId = 1;
                    }
                    if (candidateCount < 6 && maximumVertexContainmentDots.Z <= 0)
                    {
                        ref var candidate = ref candidates[candidateCount++];
                        candidate.X = projectedTangentX.Z;
                        candidate.Y = projectedTangentY.Z;
                        candidate.FeatureId = 2;
                    }
                SkipVertexCandidates:;
                }
                // 我们已经找到了这个船体槽的所有联系人。联系人数量可能超过我们需要的数量(%4),因此请执行减少操作。
                Vector3Wide.ReadSlot(ref localTriangleCenter, slotIndex, out var slotTriangleCenter);
                Vector3Wide.ReadSlot(ref triangleNormal, slotIndex, out var slotTriangleFaceNormal);
                Vector3Wide.ReadSlot(ref offsetB, slotIndex, out var slotOffsetB);
                Matrix3x3Wide.ReadSlot(ref hullOrientation, slotIndex, out var slotHullOrientation);
                ManifoldCandidateHelper.Reduce(candidates, candidateCount, slotTriangleFaceNormal, slotLocalNormal, slotTriangleCenter, hullFaceOrigin, hullFaceX, hullFaceY, epsilonScale[slotIndex], depthThreshold[slotIndex],
                   slotHullOrientation, slotOffsetB, slotIndex, ref manifold);
            }
            // 为了网格简化,将接触推到三角形。
            // 这意味着未来的船体集合边界平滑器假定船体上有接触,将不能与三角形一起正常工作,但无论如何,精细三角形应该只用于静电内容。
            // (我们这样做,而不是根据外壳平面剪裁三角形的边,因为外壳具有可变的顶点计数;将外壳顶点投影到三角形会产生更多的减少开销。)
            // 减少不指定法线。把它填进去。
            Matrix3x3Wide.TransformWithoutOverlap(localNormal, hullOrientation, out manifold.Normal);
            Vector3Wide.Scale(manifold.Normal, manifold.Depth0, out var offset0);
            Vector3Wide.Scale(manifold.Normal, manifold.Depth1, out var offset1);
            Vector3Wide.Scale(manifold.Normal, manifold.Depth2, out var offset2);
            Vector3Wide.Scale(manifold.Normal, manifold.Depth3, out var offset3);
            Vector3Wide.Subtract(manifold.OffsetA0, offset0, out manifold.OffsetA0);
            Vector3Wide.Subtract(manifold.OffsetA1, offset1, out manifold.OffsetA1);
            Vector3Wide.Subtract(manifold.OffsetA2, offset2, out manifold.OffsetA2);
            Vector3Wide.Subtract(manifold.OffsetA3, offset3, out manifold.OffsetA3);
            // 网格减少还利用特征ID中的面接触标志。
            var faceCollisionFlag = Vector.ConditionalSelect(
                Vector.LessThan(triangleNormalDotLocalNormal, new Vector<float>(-MeshReduction.MinimumDotForFaceCollision)), new Vector<int>(MeshReduction.FaceCollisionFlag), Vector<int>.Zero);
            manifold.FeatureId0 += faceCollisionFlag;
        }

        public void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex4ContactManifoldWide manifold)
        {
            throw new NotImplementedException();
        }

        public void Test(ref TriangleWide a, ref ConvexHullWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex4ContactManifoldWide manifold)
        {
            throw new NotImplementedException();
        }
    }
}
